
function MainStep4_PseudoArc
%
% 伪弧长延拓
%
% 作者：张晨
% 邮箱：chenzhang@csu.ac.cn
% 单位：中国科学院空间应用工程与技术中心，空间探索室
% 时间：2022年02月24日
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clc; clear; close all; clear path;
% restoredefaultpath;
warning off;

% 添加模块
parentFolder = fileparts(pwd);
addpath(genpath([parentFolder , '\Basic']));
addpath(genpath([parentFolder , '\Bcr4bp']));
addpath(genpath([parentFolder , '\Cr3bp']));

% 载入参数
aux = Bcr4bp_Aux;

% 载入结果
load converged_LEO2LLO;

% --------------------------------------------------------------------------------------------------------------------------
% 构造多步打靶初值
yy = getIC_Coe(converged_LEO2LLO , aux);

% 计算coe
var_tauk = Bcr4bp_xx2varEMRot2D(converged_LEO2LLO(2 : 5)' , 'leo' , aux)';
var_taukp1 = Bcr4bp_xx2varEMRot2D(converged_LEO2LLO(7 : 10)' , 'llo' , aux)';

opts = optimoptions(@fmincon , 'Algorithm' , 'active-set' , 'Display' , 'iter' , ...
    'MaxIter' , 20 , 'MaxFunEvals' , 200 , ...
    'GradObj' , 'on' , 'GradConstr' , 'on' , 'DerivativeCheck' , 'off');

% ----------------------------------------- 配置伪弧长延拓参数 -----------------------------------------
% type_Index = 1; % 约束总飞行时间
type_Index = 2; % 约束LEO发射脉冲
% type_Index = 3; % 约束LLO入轨脉冲
% type_Index = 4; % 约束LEO发射时刻（太阳相位角）
% type_Index = 5; % 约束LLO入轨时刻（太阳相位角）

% 伪弧长延拓开/关
aux.pseudoArc.IO = 'on';
% aux.pseudoArc.IO = 'off';

% 选择伪弧长延拓类型
switch type_Index
    case 1
        aux.pseudoArc.type = 'tof';
        aux.pseudoArc.desiredValue = converged_LEO2LLO(6) - converged_LEO2LLO(1);
    case 2
        aux.pseudoArc.type = 'dv_t0';
        aux.pseudoArc.desiredValue = var_tauk(3);
    case 3
        aux.pseudoArc.type = 'dv_tf';
        aux.pseudoArc.desiredValue = var_taukp1(3);
    case 4
        aux.pseudoArc.type = 'thetaS_t0';
        aux.pseudoArc.desiredValue = converged_LEO2LLO(1) * aux.EMRot.ws;
    case 5
        aux.pseudoArc.type = 'thetaS_tf';
        aux.pseudoArc.desiredValue = converged_LEO2LLO(6) * aux.EMRot.ws;
end

% 选择伪弧长延拓方向
aux.pseudoArc.dir = 'dir1';
% aux.pseudoArc.dir = 'dir2';

% 伪弧长延拓
fun_pseudoArc(yy , aux);

end
